package main;

import java.awt.Color;
import java.awt.Graphics;
import java.util.ArrayList;
//import java.util.Vector;

// Robot Model
public class Robot {

    final static double DIAMETER = 80; // Gillespie is 80cm across

 // Robot centre-of-mass
    public static Point position;
    
    private double velocityMove;   // Cms (Pixels) to move each time move() is called
    private double velocityTurn;

    // Maximum permissible x, y
    private double rightBound;
    private double bottomBound;

    // Target coordinates
    public static Point destination = new Point(340, 340);
    
    //private double target_x;
    //private double target_y;
    
    private static Vector direction;
    private Vector desiredDirection;
    
    private double tbearingx;
    private double tbearingy;
    public static ArrayList<Point> waypoints = new ArrayList<Point>();
    // Directional information
    private double angle;
    public static double bearingx; 
    public static double bearingy;

    public static Point temp = null;
    
    /*
    // Sonar variables
    private double northSonar;
    private double southSonar;
    private double eastSonar;
    private double westSonar;

    // IR variables
    private double northIR;
    private double southIR;
    private double eastIR;
    private double westIR;
    private double northEastIR;
    private double northWestIR;
    private double southEastIR;
    private double southWestIR;
    */
    public static Sensor northIR = new Sensor(false, 0, false);
    public static Sensor northEastIR = new Sensor(false, Math.PI/4, false);
    public static Sensor eastIR = new Sensor(false, Math.PI/2, false);
    public static Sensor southEastIR = new Sensor(false, 3*Math.PI/4, false);
    public static Sensor southIR = new Sensor(false, Math.PI, false);
    public static Sensor southWestIR = new Sensor(false, 5*Math.PI/4, false);
    public static Sensor westIR = new Sensor(false, 3*Math.PI/2, false);
    public static Sensor northWestIR = new Sensor(false, 7*Math.PI/4, false);
    
    public static Sensor northSonar = new Sensor(false, 0, true);
    public static Sensor eastSonar = new Sensor(false, Math.PI/2, true);
    public static Sensor southSonar = new Sensor(false, Math.PI, true);
    public static Sensor westSonar = new Sensor(false, 3*Math.PI/2, true);
   

    // Constructor
    public Robot (int x, int y, int velocityMove, int velocityTurn) {
    	
        position = new Point(x + DIAMETER/2, y + DIAMETER/2);

        // Starting velocity
        this.velocityMove = velocityMove;
        this.velocityTurn = velocityTurn;

        // Starting bearing
        this.angle = 0;
        
        this.direction = new Vector(0, DIAMETER/2);
        
        this.bearingx = DIAMETER/2;
        this.bearingy = 0;
        this.tbearingx = DIAMETER/2;
        this.tbearingy = 0;

        // Take initial sensor readings
        //getSonarReadings();
        //getInfraRedReadings();
    }

    // Set environment boundaries
    public void setBounds(int width, int height) {
        rightBound  = width - DIAMETER/2;
        bottomBound = height - DIAMETER/2;
    }

    // move() method for updating the Robot position
    public void move2() {
    	
    	if (Simulator.poi.place.distanceFromPoint(position) < 100) {
			UserInterface.simulatorPanel.infoPanel.removeAll();
			UserInterface.simulatorPanel.infoPanel.add(Simulator.poi.tooltip);
			UserInterface.simulatorPanel.infoPanel.doLayout();
			UserInterface.simulatorPanel.repaint();
		}
		
		if (Simulator.poi2.place.distanceFromPoint(position) < 100) {
			UserInterface.simulatorPanel.infoPanel.removeAll();
			UserInterface.simulatorPanel.infoPanel.add(Simulator.poi2.tooltip);
			UserInterface.simulatorPanel.infoPanel.doLayout();
			UserInterface.simulatorPanel.repaint();
		}
    	
    	
    	desiredDirection = new Vector(position, destination);    	    	   	
    	double angle = Vector.angle(direction, desiredDirection);
    	    	
    	if (Math.abs(angle) > velocityTurn && position.distanceFromPoint(destination) > 2) {
    		// if has to rotate
    		direction = direction.rotate(Math.signum(angle)*velocityTurn);
		} else if (position.x != destination.x && position.y != destination.y){ //if not at target location //movement
			
			direction = desiredDirection.unit().scale(DIAMETER/2);
			
			double gradientx = position.x - destination.x;
			double gradienty = position.y - destination.y;
			
			double norm = Math.sqrt((gradientx*gradientx) + (gradienty*gradienty));
			//position.x -= velocityMove *(gradientx/norm);
			//position.y -= velocityMove *(gradienty/norm);
			
			position.x += direction.unit().getX()*velocityMove;
			position.y += direction.unit().getY()*velocityMove;
			
    		if (position.distanceFromPoint(destination) < 3 && Math.abs(angle) < velocityTurn) {
    			position = destination;
    			if (!waypoints.isEmpty()) {
    				waypoints.remove(0); // reached a waypoint
    				if (Simulator.editing) {
	    				Simulator.closest--;
	    				if (Simulator.closest < 1) {
	    					Simulator.closest = -1;
	    					Simulator.editing = false;
	    				}
    				}
    				if (!waypoints.isEmpty()) {
    					destination = waypoints.get(0); //get a new waypoint
    				} else {
    				UserInterface.simulatorPanel.sim.setAnimation(false);
    				}
    			}
    		}
			
			/*
            //correction
    		if (Math.abs(position.x - target_x) < 1){ // not quite at target but almost
    			position.x = target_x;
    		}
    		if (Math.abs(position.y - target_y) < 1){ // not quite at target but almost
    			position.y = target_y;
    		}
    		if (position.x < DIAMETER/2) {
    			position.x = DIAMETER/2;
                //out of bounds check
            } else if (position.x > rightBound) {
            	position.x = rightBound;
            }
            if (position.y < DIAMETER/2) {
            	position.y = DIAMETER/2;

            } else if (position.y > bottomBound) {
            	position.y = bottomBound;
            }
           */
    	} else if (!waypoints.isEmpty()){
    		destination = waypoints.get(0);
    		double gradientx = position.x - destination.x;
    		double gradienty = position.y - destination.y;
    		double norm = Math.sqrt((gradientx*gradientx) + (gradienty*gradienty));        	
        	tbearingx = -(gradientx/norm)*(DIAMETER/2);
        	tbearingy = -(gradienty/norm)*(DIAMETER/2);
    	} else {
    		UserInterface.simulatorPanel.sim.timer.stop();
    	}
    	
    	UserInterface.simulatorPanel.sensorPanel.repaint();
    }
    
    // Draw method - "." shows COM, "+" shows bearing
    public void draw(Graphics g) {
    	
    	double angle;
    	
    	double x;
    	double y;
    	
    	if (northSonar.active) {
	    	angle = Math.toRadians(direction.toAngle())-northSonar.offset;
	    	x = position.x+(Math.sin(angle)*2*DIAMETER);	    	    	
	    	if (x<0) x=0;
	    	if (x>598) x=598;	    	
	    	y = position.y+(Math.cos(angle)*2*DIAMETER);	    	    	
	    	if (y<0) y=0;
	    	if (y>598) y=598;
	    	g.fillOval((int) x-2, (int) y-2, 5, 5);
	    	g.drawLine((int) x, (int) y, (int) position.x, (int) position.y);
    	}
    	if (westSonar.active) {
	    	angle = Math.toRadians(direction.toAngle())-westSonar.offset;    	
	    	x = position.x+(Math.sin(angle)*2*DIAMETER);	    	    	
	    	if (x<0) x=0;
	    	if (x>598) x=598;	    	
	    	y = position.y+(Math.cos(angle)*2*DIAMETER);	    	    	
	    	if (y<0) y=0;
	    	if (y>598) y=598;
	    	g.fillOval((int) x-2, (int) y-2, 5, 5);
	    	g.drawLine((int) x, (int) y, (int) position.x, (int) position.y);
	    }
    	if (southSonar.active) {
        	angle = Math.toRadians(direction.toAngle())-southSonar.offset;    	
        	x = position.x+(Math.sin(angle)*2*DIAMETER);	    	    	
        	if (x<0) x=0;
	    	if (x>598) x=598;	    	
	    	y = position.y+(Math.cos(angle)*2*DIAMETER);	    	    	
	    	if (y<0) y=0;
	    	if (y>598) y=598;
	    	g.fillOval((int) x-2, (int) y-2, 5, 5);
	    	g.drawLine((int) x, (int) y, (int) position.x, (int) position.y);
	    }
    	if (eastSonar.active) {
        	angle = Math.toRadians(direction.toAngle())-eastSonar.offset;    	
        	x = position.x+(Math.sin(angle)*2*DIAMETER);	    	    	
        	if (x<0) x=0;
	    	if (x>598) x=598;	    	
	    	y = position.y+(Math.cos(angle)*2*DIAMETER);	    	    	
	    	if (y<0) y=0;
	    	if (y>598) y=598;
	    	g.fillOval((int) x-2, (int) y-2, 5, 5);
	    	g.drawLine((int) x, (int) y, (int) position.x, (int) position.y);
	    }
    	
        g.fillOval((int) (position.x - DIAMETER/2), (int)(position.y - DIAMETER/2),(int) DIAMETER, (int)DIAMETER);
        g.setColor(Color.red);
        g.fillOval((int)position.x, (int)position.y, 2, 2);
        g.fillOval((int)(position.x + direction.getX()),(int)(position.y +direction.getY()), 5, 5);
        g.drawLine((int)position.x, (int)position.y, (int)destination.x, (int)destination.y);
        g.drawOval((int)destination.x-2, (int)destination.y-2, 4,4);
        if(!waypoints.isEmpty()){
        	g.drawLine((int)destination.x, (int)destination.y, (int)waypoints.get(0).getX(), (int)waypoints.get(0).getY());
        	g.drawOval((int)waypoints.get(0).getX()-2, (int)waypoints.get(0).getY()-2, 4,4);
        	for (int i = 1; i < waypoints.size(); i++){
        		g.drawLine((int)waypoints.get(i).getX(),(int)waypoints.get(i).getY(),(int)waypoints.get(i-1).getX(),(int)waypoints.get(i-1).getY());
        		g.drawOval((int)waypoints.get(i).getX()-2, (int)waypoints.get(i).getY()-2, 4,4);
        	}
        }
        
        if (temp != null) {
        	Point last = position;
        	if (!waypoints.isEmpty()) {
        		last = waypoints.get(waypoints.size()-1);
        	}
        	
        	g.drawLine((int)last.x, (int)last.y, (int)temp.x, (int)temp.y);
        }
        
        Simulator.poi.draw(g);
        Simulator.poi2.draw(g);
        
        //g.drawString("+", com_x + x_offset, com_y + y_offset);
        //g.drawString(".", com_x, com_y);
    }

    public static void drawAlone(Graphics g) {
    	g.clearRect(0, 0, 200, 200);
    	
    	double angle;
    	
    	if (northIR.active) {
        	angle = Math.toRadians(direction.toAngle())-northIR.offset;    	
        	g.fillOval(100 + (int) (Math.sin(angle)*DIAMETER-2), 100 + (int) (Math.cos(angle)*DIAMETER-2), 5, 5);
        	g.drawLine(100, 100, 100 + (int) (Math.sin(angle)*DIAMETER), 100 + (int) (Math.cos(angle)*DIAMETER));
        }
    	if (eastIR.active) {
        	angle = Math.toRadians(direction.toAngle())-eastIR.offset;    	
        	g.fillOval(100 + (int) (Math.sin(angle)*DIAMETER-2), 100 + (int) (Math.cos(angle)*DIAMETER-2), 5, 5);
        	g.drawLine(100, 100, 100 + (int) (Math.sin(angle)*DIAMETER), 100 + (int) (Math.cos(angle)*DIAMETER));
        }
    	if (westIR.active) {
        	angle = Math.toRadians(direction.toAngle())-westIR.offset;    	
        	g.fillOval(100 + (int) (Math.sin(angle)*DIAMETER-2), 100 + (int) (Math.cos(angle)*DIAMETER-2), 5, 5);
        	g.drawLine(100, 100, 100 + (int) (Math.sin(angle)*DIAMETER), 100 + (int) (Math.cos(angle)*DIAMETER));
        }
    	if (southIR.active) {
        	angle = Math.toRadians(direction.toAngle())-southIR.offset;    	
        	g.fillOval(100 + (int) (Math.sin(angle)*DIAMETER-2), 100 + (int) (Math.cos(angle)*DIAMETER-2), 5, 5);
        	g.drawLine(100, 100, 100 + (int) (Math.sin(angle)*DIAMETER), 100 + (int) (Math.cos(angle)*DIAMETER));
        }
    	if (northEastIR.active) {
        	angle = Math.toRadians(direction.toAngle())-northEastIR.offset;    	
        	g.fillOval(100 + (int) (Math.sin(angle)*DIAMETER-2), 100 + (int) (Math.cos(angle)*DIAMETER-2), 5, 5);
        	g.drawLine(100, 100, 100 + (int) (Math.sin(angle)*DIAMETER), 100 + (int) (Math.cos(angle)*DIAMETER));
        }
    	if (northWestIR.active) {
        	angle = Math.toRadians(direction.toAngle())-northWestIR.offset;    	
        	g.fillOval(100 + (int) (Math.sin(angle)*DIAMETER-2), 100 + (int) (Math.cos(angle)*DIAMETER-2), 5, 5);
        	g.drawLine(100, 100, 100 + (int) (Math.sin(angle)*DIAMETER), 100 + (int) (Math.cos(angle)*DIAMETER));
        }
    	if (southEastIR.active) {
        	angle = Math.toRadians(direction.toAngle())-southEastIR.offset;    	
        	g.fillOval(100 + (int) (Math.sin(angle)*DIAMETER-2), 100 + (int) (Math.cos(angle)*DIAMETER-2), 5, 5);
        	g.drawLine(100, 100, 100 + (int) (Math.sin(angle)*DIAMETER), 100 + (int) (Math.cos(angle)*DIAMETER));
        }
    	if (southWestIR.active) {
        	angle = Math.toRadians(direction.toAngle())-southWestIR.offset;    	
        	g.fillOval(100 + (int) (Math.sin(angle)*DIAMETER-2), 100 + (int) (Math.cos(angle)*DIAMETER-2), 5, 5);
        	g.drawLine(100, 100, 100 + (int) (Math.sin(angle)*DIAMETER), 100 + (int) (Math.cos(angle)*DIAMETER));
        }
    	
        g.fillOval(100 - (int) (DIAMETER/4), 100 - (int)( DIAMETER/4),(int) DIAMETER/2, (int)DIAMETER/2);
        g.setColor(Color.red);
        g.fillOval(100+(int)(direction.getX()/2),100+(int)(direction.getY()/2), 5, 5);
                
        //g.drawString("+", com_x + x_offset, com_y + y_offset);
        //g.drawString(".", com_x, com_y);
    }
    

    // Sensor readings are taken by compass position on the robot, and then oriented according
    // to the direction the robot is facing. Obviously this is just a hack exploiting the
    // simulated world.

    // Rough sonar distances (cm/px)
    private void getSonarDistances() {
        // North Sonar
      //  northSonar.value = distance(com_x, com_y - DIAMETER/2, com_x, 0);
        // South Sonar
        //southSonar.value = distance(com_x, com_y + DIAMETER/2, com_x, bottomBound);
        // East Sonar
        //eastSonar.value = distance(com_x + DIAMETER/2, com_y, rightBound, com_y);
        // West Sonar
        //westSonar.value = distance(com_x - DIAMETER/2, com_y, 0, com_y);
    }

    // Rough IR distances (cm/px)
    private void getInfraRedDistances() {
        // North IR
//        northIR.value = distance(com_x, com_y - DIAMETER/2, com_x, 0);
        // South IR
  //      southIR.value = distance(com_x, com_y + DIAMETER/2, com_x, bottomBound);
        // East IR
    //    eastIR.value = distance(com_x + DIAMETER/2, com_y, rightBound, com_y);
        // West IR
      //  westIR.value = distance(com_x - DIAMETER/2, com_y, 0, com_y);

        // To calculate 45 degree IRs, first take the measurement to the closest wall
        // from the COM. Then work out the hypotenuse, and subtract the distance from
        // the sensor to the COM.

        // NorthEast IR
        if (northIR.value < eastIR.value) {
            northEastIR.value = ((northIR.value + DIAMETER/2) / Math.sin(3.142/4))
                    - Math.sqrt((DIAMETER/2)*(DIAMETER/2)*2);
        } else {
            northEastIR.value = ((eastIR.value + DIAMETER/2) / Math.cos(3.142/4))
                    - Math.sqrt((DIAMETER/2)*(DIAMETER/2)*2);
        }

        // NorthWest IR
        if (northIR.value < westIR.value) {
            northWestIR.value = ((northIR.value + DIAMETER/2) / Math.sin(3.142/4))
                    - Math.sqrt((DIAMETER/2)*(DIAMETER/2)*2);
        } else {
            northWestIR.value = ((westIR.value + DIAMETER/2) / Math.cos(3.142/4))
                    - Math.sqrt((DIAMETER/2)*(DIAMETER/2)*2);
        }

        // SouthEast IR
        if (southIR.value < eastIR.value) {
            southEastIR.value = ((southIR.value + DIAMETER/2) / Math.sin(3.142/4))
                    - Math.sqrt((DIAMETER/2)*(DIAMETER/2)*2);
        } else {
            southEastIR.value = ((eastIR.value + DIAMETER/2) / Math.cos(3.142/4))
                    - Math.sqrt((DIAMETER/2)*(DIAMETER/2)*2);
        }

        // SouthWest IR
        if (southIR.value < westIR.value) {
            southWestIR.value = ((southIR.value + DIAMETER/2) / Math.sin(3.142/4))
                    - Math.sqrt((DIAMETER/2)*(DIAMETER/2)*2);
        } else {
            southWestIR.value = ((westIR.value + DIAMETER/2) / Math.cos(3.142/4))
                    - Math.sqrt((DIAMETER/2)*(DIAMETER/2)*2);
        }
    }

    // Simple triangular distance measure between two points
    private float distance(double com_x2, double d, double com_x3, double b) {
        double diffxa = com_x3 - com_x2;
        double diffyb = b - d;

        return new Float(Math.sqrt((double) (diffxa*diffxa + diffyb*diffyb)));
    }

    /*
    // Order the sonar readings depending on the bearing (which sensor reading comes 'first')
    private double[] orderSonarDistances() {
        if (direction.y == 0) {
            if (direction.x == 1) return new double[] {eastSonar, southSonar, westSonar, northSonar};
            else return new double[] {westSonar, northSonar, eastSonar, southSonar};
        } else {
            if (direction.y == 1) return new double[] {southSonar, westSonar, northSonar, eastSonar};
            else return new double[] {northSonar, eastSonar, southSonar, westSonar};
        }
    }

    // Order the IR readings
    private double[] orderInfraRedDistances() {
        if (bearing_y == 0) {
            if (bearing_x == 1) return new double[] {eastIR, southEastIR, southIR, southWestIR, westIR,
                    northWestIR, northIR, northEastIR};
            else return new double[] {westIR, northWestIR, northIR, northEastIR, eastIR, southEastIR,
                    southIR, southWestIR};
        } else {
            if (bearing_y == 1) return new double[] {southIR, southWestIR, westIR, northWestIR, northIR,
                    northEastIR, eastIR, southEastIR};
            else return new double[] {northIR, northEastIR, eastIR, southEastIR, southIR, southWestIR,
                    westIR, northWestIR};
        }
    }

    // Public method for taking sonar readings in the sensor range (rather than distance)
    public double[] getSonarReadings() {
        getSonarDistances();
        double[] distances = orderSonarDistances();
        double[] readings = distances.clone();

        for (int i = 0; i < distances.length; i++) {
            readings[i] = distances[i]/1.296;
        }

        return readings;
    }

    // Public method for taking IR readings in the sensor range (rather than distance)
    public double[] getInfraRedReadings() {
        getInfraRedDistances();
        double[] distances = orderInfraRedDistances();
        double[] readings = new double[distances.length];

        for (int i = 0; i < distances.length; i++) {

            if (distances[i] > 80) distances[i] = 80;

            if (distances[i] < 10) {
                readings[i] = distances[i]*0.02;    
            }
            else readings[i] = (4800/distances[i])+20;
        }
        
        return readings;
    }*/

    public void setWayPoint(int x, int y) {
    	setWayPoint(new Point(x,y));
    }
    
    public void setWayPoint(Point p) {
    	if (p.x >= 558 || p.y >= 558 || p.x <= 40 || p.y <= 40) {
    		Simulator.showBadMessage("Too close to boundary");
    		return;	
    	}    		
    	
    	Simulator.showGoodMessage("Waypoint added!");
    	
    	this.waypoints.add(p);
    }
    
    public static int getClosestWaypoint(Point p) {
    	
    	int closest = -1;
    	double min_dist = 1000;
    	double dist = 0;
    	
    	for (int i=0; i<waypoints.size(); i++) {
    		dist = waypoints.get(i).distanceFromPoint(p);
    		if (dist < min_dist) {
    			min_dist = dist;
    			closest = i;
    		}
    	}
    	
    	if (min_dist < 25) {
    		return closest;
    	} else {    	
    		return -1;
    	}
    }
}
